/***************************************************************************
 创建者: 　hualei
 开始时间: 2020-09-29
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 *
 * *************************************************************************
 * hualei 2020.9.29 CHARGING 充电指令类的具体实现代码编写                                                                   *
 ***************************************************************************/
#include "charginginstructionlogic.h"
#include "programset.h"
#include "programengine.h"
#include "../BatteryMonitor/batterymonitor.h"
#include "../EcatCore/robotio.h"
#include "movefreeinstructionlogic.h"
#include "MacroDefine/globalDefine.h"
#include "GeneralDefine.h"
#include "messagelog.h"
#define IF_DEBUG 0

//#define D_CHARGE_DO_INDEX 4 //从０开始
#define D_CHARGE_POSITION_FILE_PATH "globalConfig/batteryConfig/chargePosition.xml"

extern BatteryMonitor *g_batteryMonitor;

ChargingInstructionLogic::ChargingInstructionLogic(int robotIdIn, ProgramSet *programSetIn,
                         ProgramEngine *programEngineIn, RobotIo *ioControllerIn, MoveFreeInstructionLogic *moveFreeIn)
{
    robotId = robotIdIn;
    programSet = programSetIn;
    programEngine = programEngineIn;
    ioController=ioControllerIn;
    moveFree=moveFreeIn;

    QString chargePositionConfigFilePath=D_TCRCONTROLLER_DATA_PATH;
    chargePositionConfigFilePath+=D_CHARGE_POSITION_FILE_PATH;
    int tmpReturn=readConfigFile(chargePositionConfigFilePath);
    if(1!=tmpReturn)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3943, robotId);
        isInitialStatus=false;
    }
    else
    {
        isInitialStatus=true;
    }

}


E_PROGRAM_LOOP_RESULT ChargingInstructionLogic::startCharging(int programNumIn,
                                                       const ChargingInstruction &instructionIn, int lineIndex)
{

    E_PROGRAM_LOOP_RESULT tmpResult=charging_run(instructionIn,lineIndex);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
    {
        return tmpResult;
    }
    programEngine->calculateNextLineToRun(programNumIn);
    return programEngine->isTheLastInstruction(programNumIn);

}

E_PROGRAM_LOOP_RESULT ChargingInstructionLogic::charging_run(const ChargingInstruction &instructionIn, int lineIndex)
{

    if(false==isInitialStatus)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3943, robotId);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    if(instructionIn.instructionType != CHARGING)
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    else
    {
        //直接开关控制
        if(E_CHARGING_ON==instructionIn.chargingFinishedType)
        {
            chargeDeviceOn();
            return E_PROGRAM_LOOP_RESULT_NORMAL;
        }
        else if(E_CHARGING_OFF==instructionIn.chargingFinishedType)
        {
            chargeDeviceOff();
            return E_PROGRAM_LOOP_RESULT_NORMAL;
        }

        //流程控制
       E_PROGRAM_LOOP_RESULT tmpResult;
       if(1==defaultChargeType)
       {
           //先运动到充电点
           tmpResult=moveToChargePosition(true,2,lineIndex,0);//?
           if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
           {
               addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3941, robotId
                       , programEngine->getRunningLineFloat() );
               return tmpResult;
           }
           tmpResult=moveToChargePosition(true,1,lineIndex+1,barrierDetectMethod);
           if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
           {
               addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3941, robotId
                       , programEngine->getRunningLineFloat() );
               return tmpResult;
           }

           if(NULL==g_batteryMonitor || false ==g_batteryMonitor->isInitalOk())
           {
               addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3944, robotId
                       , programEngine->getRunningLineFloat() );
               return E_PROGRAM_LOOP_RESULT_ERROR;
           }
       }


        // 获取电池组状态信息
        if(g_batteryMonitor->getBatteryInfo(batteryInformation))
        {
            qDebug() << QObject::tr("ChargingInstruction ==== 电池组状态信息获取成功!");
            currentChargingInstruction = instructionIn;
        }
        else
        {
            // 电池组状态信息获取失败
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3901, robotId
                    , programEngine->getRunningLineFloat() );
            chargeDeviceOff();
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        //开始充电控制
        chargeDeviceOn();
        QTime time;
        time.start();
        int timeElapsed = time.elapsed();
        while(timeElapsed<chargeCurrencyWaitTime*1000)
        {
            g_batteryMonitor->getBatteryInfo(batteryInformation);
            usleep(10000);
            timeElapsed = time.elapsed();
            if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue() ||
                    ENUM_COMMAND_RESET == programEngine->getEngineStatue())
            {
                chargeDeviceOff();
                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
               return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
            }

        }
        if(batteryInformation.batteryCurrentCurrency>charge_maxCurrency)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3946, robotId
                    , programEngine->getRunningLineFloat(), batteryInformation.batteryCurrentCurrency,charge_maxCurrency);
            chargeDeviceOff();
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        else if(batteryInformation.batteryCurrentCurrency<charge_minCurrency)
        {
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3945, robotId
                    , programEngine->getRunningLineFloat(),batteryInformation.batteryCurrentCurrency,charge_minCurrency );
            chargeDeviceOff();
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

            // 充电时间类型
            if(instructionIn.chargingFinishedType == E_CHARGING_TIME)
            {
                QTime time;
                time.start();
                int timeElapsed = time.elapsed();
                while(timeElapsed<instructionIn.chargingTime*60*1000)
                {
                    g_batteryMonitor->getBatteryInfo(batteryInformation);
                    qtTimeSleep_second(1);
                    timeElapsed = time.elapsed();
                    if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue() ||
                            ENUM_COMMAND_RESET == programEngine->getEngineStatue())
                    {
                        chargeDeviceOff();
                        qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                       return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                    }
                }

            }
            // 充电容量类型
            else if(instructionIn.chargingFinishedType == E_CHARGING_CAPARITY)
            {
                currentChargingType = E_CHARGING_CAPARITY;
//                // 设置充电容量
//                g_batteryMonitor->setChargeVolume(instructionIn.chargingCaparity);
//                qDebug()<<instructionIn.chargingCaparity;
//                qDebug()<<batteryInformation.batteryLeftLightmodRatio*100;

                while( instructionIn.chargingCaparity > batteryInformation.batteryLeftLightmodRatio*100)
                {
                    g_batteryMonitor->getBatteryInfo(batteryInformation);
                    qtTimeSleep_second(1);
                }
                if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue() ||
                        ENUM_COMMAND_RESET == programEngine->getEngineStatue())
                {
                    chargeDeviceOff();
                    qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                   return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                }

            }
            // 选两项中最先完成的一项
            else if(instructionIn.chargingFinishedType == E_CHARGING_FIRST_FINISHED)
            {
                currentChargingType = E_CHARGING_FIRST_FINISHED;

//                g_batteryMonitor->setChargeTime(instructionIn.chargingTime);
//                g_batteryMonitor->setChargeVolume(instructionIn.chargingCaparity);
                QTime time;
                time.start();
                int timeElapsed = time.elapsed();
                while( instructionIn.chargingCaparity > batteryInformation.batteryLeftLightmodRatio*100
                      || timeElapsed<instructionIn.chargingTime*60*1000)
                {
                    g_batteryMonitor->getBatteryInfo(batteryInformation);
                    qtTimeSleep_second(1);
                    timeElapsed = time.elapsed();
                    if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue() ||
                            ENUM_COMMAND_RESET == programEngine->getEngineStatue())
                    {
                        chargeDeviceOff();
                        qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                       return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                    }
                }
            }
            else
            {
                chargeDeviceOff();
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }

            chargeDeviceOff();
            if(1==defaultChargeType)
            {
                //退出充电桩
                tmpResult=moveToChargePosition(false,2,lineIndex+2,barrierDetectMethod);
                if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
                {
                    addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3941, robotId
                            , programEngine->getRunningLineFloat() );
                    return tmpResult;
                }
            }

    }

    chargeDeviceOff();
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT ChargingInstructionLogic::charging_run(const ChargeParameter &paraIn)
{
    ChargingInstruction tmp_instruction;
    tmp_instruction.instructionType=CHARGING;
    tmp_instruction.chargingCaparity=paraIn.chargingCaparity;
    tmp_instruction.chargingTime=paraIn.chargingTime;
    tmp_instruction.chargingFinishedType=paraIn.chargingFinishedType;
    return charging_run(tmp_instruction,0);

}

E_PROGRAM_LOOP_RESULT ChargingInstructionLogic::moveToChargePosition(bool isPositiveRun,int pointIndexIn,
                                                                     int lineIndexIn,int detectBarrierMethodIn)
{
    MoveFreeInstruction tmpMoveInstruction;
    tmpMoveInstruction.instructionType=MOVE_FREE;              // 指令类型
    tmpMoveInstruction.moveFreeMode=E_MOVE_FREE_WHEEL;                 // 自由导航模式
    tmpMoveInstruction.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
    tmpMoveInstruction.pointSensorType=EM_MOVE_FREE_VMARK;
    tmpMoveInstruction.trackMoveFileName="";                      // 轨道运行文件路径
    tmpMoveInstruction.pointsValue=0;                            // DP单点数值
    tmpMoveInstruction.registerPointsValue=0;                    // DPR寄存器点数值
    tmpMoveInstruction.noteStr="";                                // 注释
    tmpMoveInstruction.CNT_Value=0;                                  // 平滑数值  当CNT_Value=0:FINE
    tmpMoveInstruction.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;
    tmpMoveInstruction.isPositiveDirection=isPositiveRun; //正向、反向运行。华磊

    tmpMoveInstruction.pointIndex=1;
    tmpMoveInstruction.laserTypeNum=1;
    tmpMoveInstruction.jmpLabel="";
    if(detectBarrierMethodIn>=0)
    {
        tmpMoveInstruction.isDetectBarrier=true; //是否探测障碍物
        tmpMoveInstruction.barrierRunMode=detectBarrierMethodIn;
    }
    else
    {
        tmpMoveInstruction.isDetectBarrier=false; //是否探测障碍物
    }

    tmpMoveInstruction.barrierRunMode=E_BARRIER_RUN_MODE_VMARK;
    if(!chargePositionList.contains(pointIndexIn))
    {
        qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3942, robotId
                , pointIndexIn);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    tmpMoveInstruction.velocityRatio=chargePositionList.value(pointIndexIn).speedRatio*100; // 运行速度比
    tmpMoveInstruction.allowAccuracy=chargePositionList.value(pointIndexIn).allowError;//停车精度，单位米
    E_PROGRAM_LOOP_RESULT tmpResult;
    tmpResult= moveFree->moveToPointByPathStyle_blockWait(tmpMoveInstruction,lineIndexIn,true,
                                     chargePositionList.value(pointIndexIn).point);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3941, robotId
                , programEngine->getRunningLineFloat() );
        return tmpResult;
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

//int ChargingInstructionLogic::moveToChargePosition()
//{
//    //设置停车精度；距离0.02时，角度3度
//    programMcController->mcControllerInterpreter->setAgvTargetAllowOffset(
//                instructionIn.allowAccuracy,2.6*instructionIn.allowAccuracy);
//    //设置是否检测障碍物
//    if(E_MOVE_FREE_MOVE_TRAJECTORY_AUTO==instructionIn.moveFreeMoveTrajectoryType)
//    {
//        programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(false);
//        programMcController->mcControllerInterpreter->setPointCloudCheckEnable(false);
//    }
//    else
//    {
//        programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(true);
//        programMcController->mcControllerInterpreter->setPointCloudCheckEnable(true);
//    }


//    programMcController->mcControllerInterpreter->setMotionServerModel
//                                         (ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);
//    //odom set
//    int tmpReturn;
//    if(0==instructionIn.moveFreeMode)
//    {
//        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_ENCODER,tmpReturn);
//    }
//    else if(2==instructionIn.moveFreeMode)
//    {
//        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_LASER,tmpReturn);
//    }


////    programMcController->mcControllerInterpreter->startOneModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);
//    //激光扫描范围
//    if(1!=programMcController->mcControllerInterpreter->setLaserIntensityFilterByType_visionMatch(instructionIn.laserTypeNum))
//    {

//        quitClear();
//        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3902, robotId
//                , programEngine->getRunningLineFloat() );
//        return E_PROGRAM_LOOP_RESULT_ERROR;
//    }

//    //vmark设置
//    if(0==instructionIn.vmarkTypeNum)
//    {
//        if(1!=programMcController->mcControllerInterpreter->changeToPositionType(E_VEHICLE_POSITION_TYPE_ROS))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3904, robotId
//                    , programEngine->getRunningLineFloat() );
//            return E_PROGRAM_LOOP_RESULT_ERROR;
//        }
//        programMcController->mcControllerInterpreter->setCheckVehiclePosition_vmark(false);
//    }
//    else
//    {
//        programMcController->mcControllerInterpreter->setCurrentVMarkType(instructionIn.vmarkTypeNum);
//        if(1!=programMcController->mcControllerInterpreter->changeToPositionType(E_VEHICLE_POSITION_TYPE_VMARK))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId
//                    , programEngine->getRunningLineFloat() );
//            return E_PROGRAM_LOOP_RESULT_ERROR;
//        }
//        programMcController->mcControllerInterpreter->setCheckVehiclePosition_vmark(true);
//    }

//    //添加轨迹指令
//    int intMotionLine= programMcController->mcControllerInterpreter->getRunningNumber_virtualLineControl();
//    if(intMotionLine!=lineIndex)
//    {
//        if(1!=processMotionInstruction(instructionIn,lineIndex))
//        {
//            quitClear();
//            qDebug()<<"startMotion error,,,";
//            return E_PROGRAM_LOOP_RESULT_ERROR;
//        }
//    }


//    //完成判断，没有则阻塞等待
//    int isFinished=-1;

//    while(-1==isFinished)
//    {
//        waitFlag=true;
//        if(1==isMotionInstructionFinished(lineIndex))
//        {
//            isFinished=1;
//        }

//        if(-1==isFinished)
//        {
//            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
//        }

//        if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue() ||
//                ENUM_COMMAND_RESET == programEngine->getEngineStatue())
//        {
//            quitClear();
//            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

//           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
//        }
//        else if(ENUM_TRAJECTORY_STOPPED==programMcController->mcControllerInterpreter->getStatus()
//                || ENUM_TRAJECTORY_CEASED==programMcController->mcControllerInterpreter->getStatus())
//        {
//            if(ENUM_COMMAND_CEASE != programEngine->getEngineStatue() &&
//                    ENUM_COMMAND_RESET != programEngine->getEngineStatue())
//            {
//                quitClear();
//                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
//                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3901, robotId
//                        , programEngine->getRunningLineFloat() );
//                return E_PROGRAM_LOOP_RESULT_ERROR;
//            }
//            programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(true);
//            programMcController->mcControllerInterpreter->setPointCloudCheckEnable(true);
//            return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
//        }
//    }
//    waitFlag=false;

//    programEngine->calculateNextLineToRun(programNumIn);
//    quitClear();
//    return programEngine->isTheLastInstruction(programNumIn);
//}

int ChargingInstructionLogic::readConfigFile(QString fileName)
{
    qDebug() << "ChargingInstructionLogic::readConfigFile ==== 初始化配置文件：" << fileName;

    DomParser domparser;
    bool ok = domparser.openXml(fileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 配置文件打开失败：" << fileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "defaultChargeType", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取defaultType结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, defaultChargeType);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "barrierDetectMethod", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取defaultType结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, barrierDetectMethod);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "charge_do_index", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取defaultType结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, charge_do_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "charge_di_index", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取defaultType结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, charge_di_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "chargeCurrencyWaitTime", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取chargeCurrencyWaitTime结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, chargeCurrencyWaitTime);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "charge_maxCurrency", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取charge_maxCurrency结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, charge_maxCurrency);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "charge_minCurrency", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取charge_minCurrency结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, charge_minCurrency);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "positionList", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "ChargingInstructionLogic::readConfigFile ==== 读取positionList结点失败：" << fileName;
        }
            return -2;
    }
    QDomNodeList posNodeList=domNode.childNodes();
    ChargeConfig tmpParameter;
    int tmpPositionIndex;
    for(int i=0;i<posNodeList.size();i++)
    {
        // 读取positionType
        domparser.readXml(domparser.findSubNode(posNodeList.at(i), "positionIndex", ok ), tmpPositionIndex);
        if( !ok )
        {
                return -3;
        }
        // 读取pos_x
        domparser.readXml(domparser.findSubNode(posNodeList.at(i), "pos_x", ok ), tmpParameter.point.posX);
        if( !ok )
        {
                return -4;
        }
        // 读取pos_y
        domparser.readXml(domparser.findSubNode(posNodeList.at(i), "pos_y", ok ), tmpParameter.point.posY);
        if( !ok )
        {
                return -5;
        }
        // 读取pos_r
        domparser.readXml(domparser.findSubNode(posNodeList.at(i), "pos_r", ok ), tmpParameter.point.posR);
        if( !ok )
        {
                return -6;
        }
        // 读取speedRatio
        domparser.readXml(domparser.findSubNode(posNodeList.at(i), "speedRatio", ok ), tmpParameter.speedRatio);
        if( !ok )
        {
                return -6;
        }
        // 读取allowError
        domparser.readXml(domparser.findSubNode(posNodeList.at(i), "allowError", ok ), tmpParameter.allowError);
        if( !ok )
        {
                return -6;
        }
        chargePositionList.insert(tmpPositionIndex,tmpParameter);
    }


    domparser.closeXml();
    return 1;

}

void ChargingInstructionLogic::qtTimeSleep_second(int second)
{
    // 处理qt事件
    QCoreApplication::processEvents();
    QThread::sleep(second);

}

int ChargingInstructionLogic::chargeDeviceOn()
{
    qDebug()<<"chargeDeviceOn charge_do_index"<<charge_do_index;
//    ioController->setDo(D_CHARGE_DO_INDEX, true);
    ioController->setDo(charge_do_index, true);
    return 1;
}

int ChargingInstructionLogic::chargeDeviceOff()
{
    qDebug()<<"chargeDeviceOff charge_do_index"<<charge_do_index;
//    ioController->setDo(D_CHARGE_DO_INDEX, false);
    ioController->setDo(charge_do_index, false);
    return 1;
}


void ChargingInstructionLogic::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotId, int parameter1, int parameter2, int parameter3, int parameter4, QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {
    case 3946:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常:实际充电电流(%1)大于最大允许充电电流(%2)！").arg(parameter2).arg(parameter3)+message;

        break;
    }
    case 3945:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常:实际充电电流(%1)小于最小允许充电电流(%2)！").arg(parameter2).arg(parameter3)+message;

        break;
    }
    case 3944:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(": 电池通讯没有启用！")+message;

        break;
    }
    case 3943:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(":充电配置文件读取失败！")+message;

        break;
    }
    case 3942:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1) +
               QObject::tr("充电停靠点没有数据！")+message;

        break;
    }
    case 3941:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（运动到充电点失败）！")+message;

        break;
    }
    case 3940:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（没有检测到充电电流）！")+message;

        break;
    }
    case 3901:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（电池组状态信息获取失败）！")+message;

        break;
    }
        default:
        {

        }
    }

    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}

void ChargingInstructionLogic::timerOutSlot()
{
    if(NULL==g_batteryMonitor || false ==g_batteryMonitor->isInitalOk())
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3944, robotId
                , programEngine->getRunningLineFloat() );
        return ;
    }

}
